#include "thermalPrototype.hpp"
//#include "Libs/SimpleJSON/JSON.hpp"
#include "MiscUtils/TdrMisc/SEngine.h"
#include <windows.h>

// https://github.com/nbsdx/SimpleJSON

using namespace std;

// +- range celcius
float randPosMinInRange(float range) {
	int randNorm = range * 100 + 1;
	int rawDat = rand() % randNorm - (randNorm - 1) / 2;
	return rawDat / 100.0f;
}

wstring formatTemp(float temp) {
	return TFormater::string2wstring(TFormater::float2str(temp, -1, 2));
}

ThermalSlave::ThermalSlave() {
	genSlaveInit();
}

ThermalSlave::ThermalSlave(ThermalSlave* ref) {
	genSlaveInit();
	genSlaveCopy(ref);

	_comm._conStat = ConnectionStatus::Missing;
	_generalStat._exeStat = ExecutionStatus::NotAvailable;
	_meta.isDiscoveryTemplate = false;
	_latestReading.init();
}

ThermalSlave::~ThermalSlave() {
	genSlaveRelease();
}

int ThermalSlave::init(GeneralSlave* ref) {
	wstring oldType = _meta.type;
	_meta = ref->_meta;
	if (oldType.length() > 0) {
		_meta.type = oldType;
	}
	_comm = ref->_comm;
	_callbacks = ref->_callbacks;
	_logger.logFilenameRef = &_generalStat.logFilename;

	_comm._conStat = ConnectionStatus::Missing;
	_generalStat._exeStat = ExecutionStatus::NotAvailable;
	_latestReading.init();
	return 1;
}

int ThermalSlave::release() {
	return 1;
}

void ThermalSlave::deactivate() {
	stopOutput();
	GeneralSlave::deactivate();
}

ThermalSlave* ThermalSlave::connectOneLikeMe() {
	ThermalSlave* newOne = new ThermalSlave(this);
	newOne->_meta.isExpecting = true;
	if (newOne->connect(1) > 0) {
		return newOne;
	}
	else {
		delete newOne;
		return nullptr;
	}
}

int ThermalSlave::configStandAloneResHeaterWithFanCooler() {
	return 1;
}

int ThermalSlave::setLocation(const char* locIn) {
	return -1;
}

int ThermalSlave::isThermalControlSupported(const char* modeNameIn) {
	return -1;
}

int ThermalSlave::logConnectStart() {
	_logMisc.connectLogBlock = makeLogBlockPtr(_meta.referenceName + L"Connect");
	return 1;
}

int ThermalSlave::logConnectSuccess() {
	writeLogLine(L"Connect success", true);
	writeLogLine(L"Detail to be added", true);
	return 1;
}

int ThermalSlave::logConnectComplete() {
	delete _logMisc.connectLogBlock;
	_logMisc.connectLogBlock = nullptr;
	return 1;
}

void ThermalSlave::updateAfterConnectAny(HandShakeProtocol* protocol) {
	stopOutput();
	GeneralSlave::updateAfterConnectAny(protocol);
}

long ThermalSlave::setTempCtrlCurve(float* tempcelsiuses, float* rampRates, int* holdSeconds, int pointNum, float surrounding) {
	LogBlockCtrl logBlock = makeLogBlock(L"Set temperature control curve for " + _meta.referenceName);

	abortSchedule();

	_ctrlCurve.pts.resize(pointNum);
	long totalSeconds = 0;
	float temp = 25;
	if (pointNum <= 0) {
		string logLine = "No point on curve. Will monitor.";
		writeLogLine(logLine, true);
		TLogger::writeLine(logLine);
	}
	else {
		string logLine;
		for (int pointIdx = 0; pointIdx < pointNum; pointIdx++) {
			string logLine = "Point #" + TFormater::int2str(pointIdx);
			logLine += ", ramp by " + TFormater::int2str(rampRates[pointIdx]);
			logLine += ", then hold " + TFormater::int2str(holdSeconds[pointIdx]);
			logLine += "s @ " + TFormater::float2str(tempcelsiuses[pointIdx], -1, 2);
			writeLogLine(logLine, true);
			TLogger::writeLine(logLine);
			auto& pt = _ctrlCurve.pts[pointIdx];
			pt.tempCelcius = tempcelsiuses[pointIdx];
			pt.rampRate = rampRates[pointIdx];
			pt.holdSeconds = holdSeconds[pointIdx];
			if (pt.rampRate == 0) {
				if (pt.tempCelcius < surrounding) {
					pt.rampRate = -10;
				}
				else {
					pt.rampRate = 10;
				}
			}
			totalSeconds += abs((pt.tempCelcius - temp) / pt.rampRate);

			totalSeconds += pt.holdSeconds;
			temp = pt.tempCelcius;
		}
	}
	_ctrlCurve.currentSection = -1;
	_ctrlCurve.currentSubSection = -1;
	_ctrlCurve.pausedSection = 0;
	_ctrlCurve.pausedSubSection = 0;

	activate();
	_generalStat.scheduleSetTime = clock();
	return totalSeconds;
}

long ThermalSlave::setTempCtrlCurve(TempCtrlCurveRL& curveRef, float surrounding) {
	{
		TLogger block("Remove current curve for new one");

		_lastCurve = _ctrlCurve;
		abortSchedule();
	}

	long totalSeconds = 0;
	float temp = 25;

	{
		TLogger block("Set new curve");
		for (int repIdx = 0; repIdx < curveRef.repetitions; repIdx++) {
			for (auto& pt : curveRef.curve.pts) {
				if (pt.rampRate == 0) {
					// should not happen
					bool coolSet = false;
					if (pt.tempCelcius < surrounding) {
						coolSet = true;
					}
					else {
						if (_thermalMisc.lastTempSet > -300) {
							if (pt.tempCelcius < _thermalMisc.lastTempSet) {
								coolSet = true;
							}
						}

					}

					if (coolSet) {
						TLogger::write(L"Cool");
						pt.rampRate = -1;
					}
					else {
						TLogger::write(L"Heat");
						pt.rampRate = 1;
					}
				}
				_ctrlCurve.pts.push_back(pt);
				if (pt.rampRate != 0) {
					totalSeconds += abs((pt.tempCelcius - temp) / pt.rampRate);
				}
				TLogger::write(" to " + TFormater::float2str(pt.tempCelcius));
				TLogger::writeLine(" for " + TFormater::int2str(pt.holdSeconds) + " seconds");
				totalSeconds += pt.holdSeconds;
				temp = pt.tempCelcius;
				_thermalMisc.lastTempSet = temp;
			}
		}

		_ctrlCurve.currentSection = -1;
		_ctrlCurve.currentSubSection = -1;
		_ctrlCurve.pausedSection = 0;
		_ctrlCurve.pausedSubSection = 0;
	}

	activate();
	_generalStat.scheduleSetTime = clock();
	return totalSeconds;
}

long ThermalSlave::autoAdjustTempCtrlCurve(TempCtrlCurveRL& curveRef, float surrounding) {
	float temp = 25;

	{
		TLogger block("Adjust one curve segment");
		for (int repIdx = 0; repIdx < curveRef.repetitions; repIdx++) {
			for (auto& pt : curveRef.curve.pts) {
				if (pt.rampRate == 0) {
					bool coolSet = false;
					if (pt.tempCelcius < surrounding) {
						coolSet = true;
						TLogger::write("Cool (segment under surrounding)");
					}
					else {
						if (_thermalMisc.lastTempSet > -300) {
							if (pt.tempCelcius < _thermalMisc.lastTempSet) {
								coolSet = true;
								TLogger::write("Cool (segment lower than last)");
							}
						}
					}

					if (coolSet) {
						pt.rampRate = -5;
					}
					else {
						TLogger::write(L"Heat");
						pt.rampRate = 5;
					}
				}
				else if (pt.rampRate > 0) {
					TLogger::write(L"Heat");
				}
				else {
					TLogger::write(L"Cool");
				}
				_ctrlCurve.pts.push_back(pt);
				TLogger::write(" to " + TFormater::float2str(pt.tempCelcius));
				TLogger::writeLine(" for " + TFormater::int2str(pt.holdSeconds) + " seconds");
				temp = pt.tempCelcius;
				_thermalMisc.lastTempSet = temp;
			}
		}
	}

	return 0;
}

int ThermalSlave::startSchedule() {
	auto content = wstring(L"Activate temperature curve of " + _meta.referenceName);
	writeLogLine(content, true);
	TLogger::writeLine(content);
	_ctrlCurve.currentSection = _ctrlCurve.pausedSection;
	_ctrlCurve.currentSubSection = 0;
	_ctrlCurve.excutionTimeSumMs = 0;

	if (_ctrlCurve.currentSection >= _ctrlCurve.pts.size()) {
		auto content = L"But all sections completed";
		writeLogLine(content, true);
		TLogger::writeLine(content);
		return 0;
	}

	activate();
	return activateTempCtrlCurveSection();
	//if (_comm.connectionSubType.compare(L"simulated") == 0) {
	//	writeLogLine("Activated(Simu)", true);
	//	return 1;
	//}
	//else {
	//}
}

int ThermalSlave::activateTempCtrlCurveSection() {
	// M104 mode,temp,rate,duration\r\n
//   mode: heat or cool
//   temp: float celcius
//   rate: float celcius per second
//   duration: integer millisecond
//   M104 heat,50,2,0\r\n
// Plate {%s} towards {%.1f} at {%.1f}dps, then hold for {%d}s\r\n
	if (isSimu()) {
		_thermalSimu.section = _ctrlCurve.currentSection;
		_thermalSimu.subSection = 0;
		return 1;
	}

	string commandStr = "M104";
	TempCurvePt& pt = _ctrlCurve.pts[_ctrlCurve.currentSection];
	if (pt.rampRate > 0) {
		commandStr.append(" heat");
	}
	else {
		commandStr.append(" cool");
	}
	commandStr.append(","); commandStr.append(TFormater::float2str(pt.tempCelcius, -1, 2));
	commandStr.append(","); commandStr.append(TFormater::float2str(abs(pt.rampRate), -1, 2));
	commandStr.append(","); commandStr.append(TFormater::int2str(pt.holdSeconds));
	commandStr.append("\r\n");

	int ret = sendString(commandStr);
	if (ret > 0) {
		ret = receive(nullptr);
	}

	if (ret > 0) {
		_logMisc.sectionLogBlock = makeLogBlockPtr(L"Schedule section " + TFormater::int2wstr(_ctrlCurve.currentSection));
		_ctrlCurve.currentSubSection = 0;
		_logMisc.subsectionLogBlock = makeLogBlockPtr(L"Sub-section " + TFormater::int2wstr(_ctrlCurve.currentSubSection));
	}
	else {
	}

	return ret;
}

int ThermalSlave::readOut(uint64_t index) {
	if (_meta.isSimulated > 0) {
		return simuReadOut();
	}
	string commandStr = "?\r\n";
	writeLogLine("Read out", true);

	if (sendString(commandStr) > 0) {
		string reply;
		int ret = receive(&reply);
		if (ret > 0) {
			if (!_latestReading.fromStr(reply)) {
				writeLogLine("Parse failed!");
				return -1;
			}
			else {
				_latestReading.readOutTimeMs = clock();

				records.push_back(_latestReading);
				return 1;
			}
		}
		else {
			writeLogLine("Receive failed!");
		}
		return ret;
	}
	return -1;
}

int ThermalSlave::updateScheduleProgress(int attemptToPause) {
	if (_ctrlCurve.pts.size() == 0) {
		return -1;
	}
	if (_ctrlCurve.currentSection < 0) {
		return -1;
	}
	if (_ctrlCurve.currentSection == _ctrlCurve.pts.size()) {
		return -1;
	}

	char idle[] = "Idle";
	char hold[] = "Hold";

	bool isSectionComplete = (strcmp(_latestReading.stateOutputMode.c_str(), idle) == 0);
	bool isSubsection1 = (strcmp(_latestReading.stateDriveMode.c_str(), hold) == 0);
	writeLogLine("Update schedule", true);

	if (isSectionComplete) {
		if (attemptToPause > 0) {
			//TLogger::writeLine("Noticed attempt to pause");
			if (isSubsection1) {
				//TLogger::writeLine("Is section 1. Pause.");
				return 0;
			}
			else {
				//TLogger::writeLine("Not section 1 yet. Wait.");
			}
		}

		delete _logMisc.sectionLogBlock;
		_logMisc.sectionLogBlock = nullptr;
		delete _logMisc.subsectionLogBlock;
		_logMisc.subsectionLogBlock = nullptr;

		_ctrlCurve.excutionTimeSumMs += _latestReading.executionTimeMs;
		string logLine = "Section total execution time till now: " + TFormater::int2str(_ctrlCurve.excutionTimeSumMs);
		writeLogLine(logLine, true);

		_ctrlCurve.currentSection++;

		if (_ctrlCurve.currentSection == _ctrlCurve.pts.size()) {
			_thermalSimu.section = -1;
			writeLogLine("All schedule complete", true);
			//writeRecordToCsvFile(true);
			return -1;
		}
		else {
			_ctrlCurve.currentSubSection = 0;
			activateTempCtrlCurveSection();
			return 1;
		}
	}
	else {
		if (isSubsection1 && (_ctrlCurve.currentSubSection != 1)) {
			_ctrlCurve.currentSubSection = 1;
			_logMisc.subsectionLogBlock = makeLogBlockPtr(L"Sub-section " + TFormater::int2wstr(_ctrlCurve.currentSubSection));
		}
		return 1;
	}
}

int ThermalSlave::abortSchedule() {
	int ret = 1;
	_thermalSimu.section = -1;
	TLogger::writeLine(getReferenceName() + L" abort temp curves");

	ret = stopOutput();
	if (_ctrlCurve.pts.size() > 0) {
		_lastCurve = _ctrlCurve;
		_ctrlCurve.reset();
	}

	deactivate();
	_generalStat.scheduleSetTime = 0;

	return ret;
}

void ThermalSlave::protocolStartReset() {
	_thermalMisc.lastTempSet = -300;
}

void ThermalSlave::recordReading() {
	records.push_back(_latestReading);
	if (_simuMeta.simuStartTime != 0) {
		records.back().readOutTimeMs -= _simuMeta.simuStartTime;
	}
}

void ThermalSlave::setNewRecordFile(std::wstring filename) {
	records.clear();
	_generalStat.recordFilename = filename;
	_meta.isRecordFileWriten = false;
	TLogger::writeLine(L"Record file: " + filename);
}

int ThermalSlave::writeToRecordFile(bool newFile) {
	if (_generalStat.recordFilename.length() == 0) {
		return 0;
	}
	if (_meta.isRecordFileWriten) {
		TLogger::writeLine("Log has been written");
		return 0;
	}

	TWrite2Txt writer(_generalStat.recordFilename, true);
	if (newFile) {
		string titleLine = "time_stamp,heat_bed,container,pump,core,surrounding";
		for (int idx = 0; idx < _latestReading.additionalTemps.size(); idx++) {
			titleLine.append(",sensor_" + TFormater::int2str(idx));
		}
		titleLine.append("\n");
		writer.writeString(titleLine);
	}
	for (Readings& rd : records) {
		string line = TFormater::int2str<clock_t>(rd.readOutTimeMs, 9);
		line.append(","); line.append(TFormater::float2str(rd.heatBedTemp, -1, 2));
		line.append(","); line.append(TFormater::float2str(rd.containerTemp, -1, 2));
		line.append(","); line.append(TFormater::float2str(rd.pumpTemp, -1, 2));
		line.append(","); line.append(TFormater::float2str(rd.controllerTemp, -1, 2));
		line.append(","); line.append(TFormater::float2str(rd.surroundingTemp, -1, 2));
		for (float& temp : rd.additionalTemps) {
			line.append(","); line.append(TFormater::float2str(temp, -1, 2));
		}
		line.append("\n");
		writer.writeString(line);
	}

	TLogger::writeLine(getReferenceName() + L" log written to " + _generalStat.recordFilename);
	_meta.isRecordFileWriten = true;

	return 1;
}

int ThermalSlave::setOnlineSimu(int flag) {
	if (isConnected() <= 0) {
		return -1;
	}
	string commandStr = "dbg_simu " + TFormater::int2str(flag) + "\r\n";
	if (sendString(commandStr) > 0) {
		string reply;
		int ret = receive(&reply);
		return ret;
	}
	return -1;
}

int ThermalSlave::setSpOb(int flag) {
	string commandStr = "config flag SpOb " + TFormater::int2str(flag) + "\r\n";
	if (sendString(commandStr) > 0) {
		string reply;
		int ret = receive(&reply);
		return ret;
	}
	return -1;
}

int ThermalSlave::simuReadOut() {
	if (_thermalSimu.section == -1) {
		_latestReading.randFromRoom25C();
	}
	else {
		//cout << "Simu ReadOut: Current section: " << _ctrlCurve.currentSection << ", current sub: " << _ctrlCurve.currentSubSection << endl;
		if (_thermalSimu.section < _ctrlCurve.pts.size()) {
			if (_thermalSimu.section > 0) {
				int stop = 1;
			}
			if (_thermalSimu.subSection == 0) {
				if (_ctrlCurve.pts[_thermalSimu.section].rampRate > 0) {
					_latestReading.stateOutputMode = "Heat";
				}
				else if (_ctrlCurve.pts[_thermalSimu.section].rampRate < 0) {
					_latestReading.stateOutputMode = "Cool";
				}

				int isTargetReached = _latestReading.randDriveTowards(
					_ctrlCurve.pts[_thermalSimu.section].tempCelcius,
					_ctrlCurve.pts[_thermalSimu.section].rampRate,
					_thermalSimu.updateRate,
					10);
				if (isTargetReached > 0) {
					TLogger::writeLine("Charge target reached. Sub section hold on. ");
					_simuStat.simuCurvePtReachedTime = clock();
					_thermalSimu.subSection++;
					_latestReading.stateDriveMode = "Hold";
				}
				else {
					_latestReading.stateDriveMode = "Chrg";
				}
			}
			else if (_thermalSimu.subSection == 1) {
				_latestReading.randDriveTowards(
					_ctrlCurve.pts[_thermalSimu.section].tempCelcius,
					_ctrlCurve.pts[_thermalSimu.section].rampRate,
					_thermalSimu.updateRate,
					10);
				clock_t timeElapsedSinceReachTargetSec = (_latestReading.readOutTimeMs - _simuStat.simuCurvePtReachedTime) * _thermalSimu.updateRate / 1000;
				if (timeElapsedSinceReachTargetSec >= _ctrlCurve.pts[_thermalSimu.section].holdSeconds) {
					TLogger::writeLine("Hold done. Start: " + TFormater::int2str(_simuStat.simuCurvePtReachedTime)
						+ ". Now: " + TFormater::int2str(_latestReading.readOutTimeMs)
						+ ". Hold: " + TFormater::int2str(_ctrlCurve.pts[_thermalSimu.section].holdSeconds));
					_thermalSimu.subSection = -1;
					_latestReading.stateOutputMode = "Idle";
					_latestReading.stateDriveMode = "Hold";
				}
			}
		}
		else {
			_latestReading.randDropTowardsRoom25C(_thermalSimu.updateRate);
			_latestReading.stateOutputMode = "Idle";
			_latestReading.stateDriveMode = "Hold";
		}
	}
	//SLogger::writeLine("simuReadOut: new thermal temp: " + TFormater::float2str(_latestReading.heatBedTemp));
	Sleep(max(1, 50 / _thermalSimu.updateRate));
	return 1;
}

int ThermalSlave::simuStart(float rate) {
	_thermalSimu.updateRate = rate;
	_simuMeta.simuStartTime = clock();
	_latestReading.randFromRoom25C();
	return 1;
}

clock_t ThermalSlave::getReadings(float* heatBed, float* container, float* pump, float* controller, float* surrounding, std::vector<float>* additional) {
	clock_t ret;
	if (_simuMeta.simuStartTime > 0) {
		ret = _latestReading.readOutTimeMs - _simuMeta.simuStartTime;
	}
	else {
		ret = _latestReading.executionTimeMs + _ctrlCurve.excutionTimeSumMs;
	}

	writeLogLine("Get readings T" + TFormater::int2str(ret), true);
	ProcS::setFltIfNotNullPtr(_latestReading.heatBedTemp, heatBed);
	ProcS::setFltIfNotNullPtr(_latestReading.containerTemp, container);
	ProcS::setFltIfNotNullPtr(_latestReading.pumpTemp, pump);
	ProcS::setFltIfNotNullPtr(_latestReading.controllerTemp, controller);
	ProcS::setFltIfNotNullPtr(_latestReading.surroundingTemp, surrounding);
	ProcS::setIfNotNullPtr<vector<float>>(_latestReading.additionalTemps, additional);

	return ret;
}

void ThermalSlave::getScheduleProgress(int* sectionIdx, int* subsectionIdx, int* sectionNum, float* targetTemp) {
	*sectionIdx = _ctrlCurve.currentSection;
	*subsectionIdx = _ctrlCurve.currentSubSection;
	*sectionNum = _ctrlCurve.pts.size();
	if ((_ctrlCurve.pts.size() > 0) && (_ctrlCurve.currentSection >= 0)) {
		*targetTemp = _ctrlCurve.pts[min((int)_ctrlCurve.pts.size() - 1, _ctrlCurve.currentSection)].tempCelcius;
	}
	else {
		*targetTemp = -101;
	}
}

int ThermalSlave::getScheduleProgressX(int exeMin, int exeMax) {
	if (_ctrlCurve.pts.size() == 0) {
		return -1;
	}
	else if (_ctrlCurve.currentSection == _ctrlCurve.pts.size()) {
		return 100;
	}
	else {
		float ptNum = _ctrlCurve.pts.size();
		float percentPerPt = 100 / ptNum;
		int ret = percentPerPt * _ctrlCurve.currentSection;
		ret = max(exeMin, min(exeMax, ret));
		return ret;
	}
}

int ThermalSlave::stopOutput() {
	if (isConnected() <= 0) {
		return 1;
	}
	string commandStr = "M18\n";
	int ret = T_AUTOMA_SLAVE_ERROR_NOT_RESPONSIVE;
	int reply = sendString(commandStr);
	if (reply > 0) {
		// All off \r\n
		reply = receive(nullptr);
		if (reply > 0) {
			ret = 1;
		}
	}
	return ret;
}

std::wstring ThermalSlave::summarizeSchedule(int indent) {
	wstring ret;
	wstring indentStr = L"";
	for (int idx = 0; idx < indent; idx++) {
		indentStr += L" ";
	}
	wstring moreIndentStr = indentStr;
	for (int idx = 0; idx < 2; idx++) {
		moreIndentStr += L" ";
	}

	ret += indentStr + L"Slave name: " + _meta.referenceName + L"\n";
	ret += indentStr + L"Model: " + _meta.model + L"\n";
	ret += indentStr + L"Simu: " + TFormater::int2wstr(_meta.isSimulated) + L"\n";
	if (_meta.isSimulated > 0) {
		if (_meta.isSimuSpecified) {
			// not implemented
		}
		else {
			ret += indentStr + L"Simurate (global): " + TFormater::float2wstr(_thermalSimu.updateRate) + L"\n";
		}
	}
	ret += indentStr + L"Log path: " + _generalStat.logFilename + L"\n";
	ret += indentStr + L"Rec path: " + _generalStat.recordFilename + L"\n";
	ret += indentStr + L"Schedule: ";

	if (_ctrlCurve.pts.size() == 0) {
		ret += L"None\n";
	}
	else {
		ret += L""
			+ TFormater::int2wstr(_ctrlCurve.currentSection) + L"-" + TFormater::int2wstr(_ctrlCurve.currentSubSection)
			+ L" / "
			+ TFormater::int2wstr(_ctrlCurve.pts.size())
			+ L" @"
			+ TFormater::int2wstr(_ctrlCurve.excutionTimeSumMs) + L"\n";
		for (int ptIdx = 0; ptIdx < _ctrlCurve.pts.size(); ptIdx++) {
			ret += moreIndentStr + L"P #" + TFormater::int2wstr(ptIdx) + L", " + _ctrlCurve.pts[ptIdx].toStr() + L"\n";
		}
	}

	return ret;
}

int ThermalSlave::queueCommand(std::wstring cmd, int para0) {
	string stdCmd = TFormater::wstring2string(cmd);
	if (stdCmd.compare("identify") == 0) {
		TLogger::writeLine("Queue identify command");
		if (para0 == 0) {
			queueCommandBase("M18\n", 1);
		}
		else {
			queueCommandBase("identify \q\n", 0);
		}
		return 1;
	}
	return 0;
}

int ThermalSlave::queueSetCommand(std::wstring cmd, std::wstring para0) {
	string stdCmd = TFormater::wstring2string(cmd);
	string stdPara0 = TFormater::wstring2string(para0);
	if (stdCmd.compare("descriptor.identifier") == 0) {
		_comm.identifier = para0;
		queueCommandBase("config meta id " + stdPara0, 3);
		return 1;
	}
	return 0;
}

std::wstring ThermalSlave::getOutputModeMajor() {
	if (_latestReading.stateOutputMode.compare("Idle") == 0) {
		return TFormater::string2wstring(_latestReading.stateOutputMode);
	}
	if (_latestReading.stateOutputMode.compare("Unknown") == 0) {
		return TFormater::string2wstring(_latestReading.stateOutputMode);
	}
	return TFormater::string2wstring(_latestReading.stateOutputMode + "ing");
}

std::wstring ThermalSlave::getOutputModeMinor() {
	return L" ";
}

void ThermalSlave::Readings::init() {
	stateOutputMode = "Unknown";
	readOutTimeMs = 0;
	additionalTemps.resize(3, 0);
}

bool ThermalSlave::Readings::fromStr(std::string& reply) {
	// Idle-Hold by{000}|{000}@{  25.1},Plt{40.04}to{40.0},Lid{41.35}to{41.3},Sur{38.0},Cor{40.1},Fan0@{000}rps,Fan1@{000}rps
	TFormater parser(reply);
	if (parser.read(4, stateOutputMode) == 0) {
		return false;
	}
	if (parser.moveBy(1) == 0) {
		return false;
	}
	if (parser.read(4, stateDriveMode) == 0) {
		return false;
	}
	float excTime;
	if (parser.parseFltBetween("@{", "}", excTime) == 0) {
		return false;
	}
	executionTimeMs = round(excTime * 1000);
	if (parser.parseFltBetween("Plt{", "}", heatBedTemp) == 0) {
		return false;
	}
	if (parser.parseFltBetween("Ctn{", "}", containerTemp) == 0) {
		return false;
	}
	if (parser.parseFltBetween("Pmp{", "}", pumpTemp) == 0) {
		return false;
	}
	if (parser.parseFltBetween("Sur{", "}", surroundingTemp) == 0) {
		return false;
	}
	if (parser.parseFltBetween("Cor{", "}", controllerTemp) == 0) {
		return false;
	}
	return true;
}

void ThermalSlave::Readings::randFromRoom25C(unsigned int seed) {
	if (seed > 0) {
		srand(seed);
	}
	heatBedTemp = 25 + randPosMinInRange(3);
	containerTemp = heatBedTemp + randPosMinInRange(1);
	pumpTemp = heatBedTemp + randPosMinInRange(2);
	controllerTemp = heatBedTemp + randPosMinInRange(3);
	surroundingTemp = 25 + randPosMinInRange(3);
	for (float& exTemp : additionalTemps) {
		exTemp = 25 + randPosMinInRange(3);
	}

	readOutTimeMs = clock();
}

float driveLimit(float value, float lim) {
	if (lim == 0) {
		return value;
	}
	else if (value > 0) {
		return min(value, lim);
	}
	return max(value, -lim);
}

int ThermalSlave::Readings::randDriveTowards(float targetTemp, float rampUpRate, int timeElapseRate, float changeLimit) {
	TLogger::write("Target: " + TFormater::float2str(targetTemp, -1, 1) + ". ");
	TLogger::write("Ramp: " + TFormater::float2str(rampUpRate, -1, 1) + ". ");
	TLogger::write("Current: " + TFormater::float2str(heatBedTemp, -1, 2) + ". ");

	clock_t realTime = clock();
	float realTimeDiff = (realTime - readOutTimeMs) * timeElapseRate / 1000.0f;
	// no more than 1s, to avoid err
	realTimeDiff = min(realTimeDiff, 1);
	float expTempDiff = rampUpRate * realTimeDiff;
	int ret = simulateReading(heatBedTemp, targetTemp, rampUpRate, driveLimit(expTempDiff, changeLimit), 0.5 * realTimeDiff);
	//simulateReading(coverTemp, rampUpRate, expTempDiff, 0.5);

	const float controllerExp = 50;
	const float surroundingExp = 40;
	const float controllerRampUp = 0.5;
	const float surroundingRampUp = 0.2;
	simulateReading(controllerTemp, controllerExp, controllerRampUp, controllerRampUp * realTimeDiff, 0.5 * realTimeDiff);
	simulateReading(surroundingTemp, surroundingExp, surroundingRampUp, surroundingRampUp * realTimeDiff, 0.5 * realTimeDiff);
	for (float& exTemp : additionalTemps) {
		simulateReading(exTemp, surroundingExp, controllerRampUp, controllerRampUp * realTimeDiff, 0.5 * realTimeDiff);
	}

	TLogger::writeLine("New: " + TFormater::float2str(heatBedTemp, -1, 2) + ". ");
	readOutTimeMs = realTime;
	return ret;
}

//clock_t ThermalSlave::Readings::randDriveHold(float targetTemp, int timeElapseRate) {
//
//	return 1;
//}

void ThermalSlave::Readings::randDropTowardsRoom25C(int timeElapseRate) {
	clock_t realTime = clock();
	float realTimeDiff = (realTime - readOutTimeMs) * timeElapseRate / 1000.0f;

	const float target = 25;
	const float heatBedRampUp = 0.8;
	const float controllerRampUp = 0.5;
	const float surroundingRampUp = 0.2;
	simulateReading(heatBedTemp, target, 0, heatBedRampUp * realTimeDiff, 0.75 * realTimeDiff);
	simulateReading(controllerTemp, target, 0, controllerRampUp * realTimeDiff, 0.75 * realTimeDiff);
	simulateReading(surroundingTemp, target, 0, surroundingRampUp * realTimeDiff, 0.75 * realTimeDiff);

	readOutTimeMs = realTime;
}

int ThermalSlave::Readings::simulateReading(float& temp, float target, float rampRate, float expTempDiff, float rangeRange) {
	if (rampRate > 0) {
		if (temp + expTempDiff >= target) {
			temp = target + randPosMinInRange(rangeRange);
			return 1;
		}
		else {
			temp += expTempDiff + randPosMinInRange(rangeRange);
			return 0;
		}
	}
	else if (rampRate < 0) {
		if (temp + expTempDiff <= target) {
			temp = target + randPosMinInRange(rangeRange);
			return 1;
		}
		else {
			temp += expTempDiff + randPosMinInRange(rangeRange);
			return 0;
		}
	}
	else {
		// rampRate == 0, converge towards target
		// expTempDiff should always be + as a negative is meaningless
		if (temp > target) {
			return simulateReading(temp, target, -expTempDiff, -expTempDiff, rangeRange);
		}
		else {
			return simulateReading(temp, target, expTempDiff, expTempDiff, rangeRange);
		}
	}
}

void ThermalSlave::writeLogLine(std::wstring line, bool withTime) {
	writeLogLinePlain(_logMisc.groupIndentStr + line, withTime, 0);
}

void ThermalSlave::writeLogLine(std::string line, bool withTime) {
	writeLogLine(TFormater::string2wstring(line), withTime);
}

ThermalSlave::LogBlockCtrl ThermalSlave::makeLogBlock(const std::wstring& blockName) {
	LogBlockCtrl ret(_logMisc.getIndentRef(), blockName, &_logger);
	return ret;
}

ThermalSlave::LogBlockCtrl* ThermalSlave::makeLogBlockPtr(const std::wstring& blockName) {
	LogBlockCtrl* ret = new LogBlockCtrl(_logMisc.getIndentRef(), blockName, &_logger);
	return ret;
}
